// Copyright (c) 2022 ChenJun
// Licensed under the Apache-2.0 License.

#ifndef RM_SERIAL_DRIVER__PACKET_HPP_
#define RM_SERIAL_DRIVER__PACKET_HPP_

#include <algorithm>
#include <cstdint>
#include <vector>
#include "rm_serial_driver/SerialEncoder.hpp"

namespace rm_serial_driver
{
struct ReceivePacket
{
  uint8_t header = 0x5A;
  uint8_t detect_color : 1;  // 0-red 1-blue
  bool reset_tracker : 1;
  uint8_t reserved : 6;
  float roll;
  float pitch;
  float yaw;
  float aim_x;
  float aim_y;
  float aim_z;
  uint16_t checksum = 0;
} __attribute__((packed));

// struct SendPacket
// {
//   uint8_t header = 0xA5;
//   bool tracking : 1;
//   uint8_t id : 3;          // 0-outpost 6-guard 7-base
//   uint8_t armors_num : 3;  // 2-balance 3-outpost 4-normal
//   uint8_t reserved : 1;
//   float x;
//   float y;
//   float z;
//   float yaw;
//   float vx;
//   float vy;
//   float vz;
//   float v_yaw;
//   float r1;
//   float r2;
//   float dz;
//   uint16_t checksum = 0;
// } __attribute__((packed));
// struct SendPacket
// {
//   bool Isshoot = 0;
//   float dyaw;
//   float dpitch;
//   uint16_t checksum = 0;
// } __attribute__((packed));

struct SendCanPacket {
  uint8_t header = 0xaa;
  uint8_t mode = 0xc8;
  uint8_t id_lsb;
  uint8_t id_msb;
  ctr_data data;
  uint8_t end = 0x55;
} __attribute__((packed));

inline ReceivePacket fromVector(const std::vector<uint8_t> & data)
{
  ReceivePacket packet;
  std::copy(data.begin(), data.end(), reinterpret_cast<uint8_t *>(&packet));
  return packet;
}

// inline std::vector<uint8_t> toVector(const SendPacket & data)
// {
//   std::vector<uint8_t> packet(sizeof(SendPacket));
//   std::copy(
//     reinterpret_cast<const uint8_t *>(&data),
//     reinterpret_cast<const uint8_t *>(&data) + sizeof(SendPacket), packet.begin());
//   return packet;
// }

inline std::vector<uint8_t> toVector(const SendCanPacket & data)
{
  std::vector<uint8_t> packet(sizeof(SendCanPacket));
  std::copy(
    reinterpret_cast<const uint8_t *>(&data),
    reinterpret_cast<const uint8_t *>(&data) + sizeof(SendCanPacket), packet.begin());
  return packet;
}

}  // namespace rm_serial_driver

#endif  // RM_SERIAL_DRIVER__PACKET_HPP_
